// Standard includes
#include <math.h>
#include <fstream>

#ifndef _IEEE_H_
#define _IEEE_H_

// Contants
#define PI 3.1415926535897932384626433832795

// Typedefs
typedef unsigned char Byte;

// Function prototypes
double convertToDecimal(Byte *, int, int);
double convertIEEE32ToDec(char *);
double convertBinToDec(char *, int, int);
void convertHexToBin(char *, char *, int);
char convertIntToChar(int);

// convertToDecimal
// Convert the raw response from the IMU to a decimal double
double convertToDecimal(Byte *response, int start, int size) {
	char bytes[9];

	for(int i=start; i<start+size; i++) {
		int d = (int)(response[i] | 0x00);
		bytes[2*i-(2*start)] = convertIntToChar(d >> 4);
		bytes[2*i-(2*start-1)] = convertIntToChar(d & 0x0f);
	}
	bytes[8] = '\0';

	return convertIEEE32ToDec(bytes);
}

// convertIEEE32ToDec
// Convert a 32-bit hexadecimal IEEE floating point string into a decimal double
double convertIEEE32ToDec(char *toConvert) {
	// Convert hex to binary
	char binary[33];
	convertHexToBin(toConvert, binary, 8);
	binary[32] = '\0';

	// Sign bit
	double sign = (binary[0] == '0' ? 1.0 : -1.0);

	// Exponent bits
	char e[9];
	for(int i=1; i<9; i++) {
		e[i-1] = binary[i];
	}
	e[8] = '\0';
	double exp = convertBinToDec(e, 8, 0) - 127;

	// Fraction bits
	char f[24]; 
	int i;
	for(i=9; i<32; i++) {
		f[i-9] = binary[i];
	}
	f[23] = '\0';
	double frac = 1 + convertBinToDec(f, 0, 23);

	return (sign * pow(2, exp) * frac);
}

// convertBinToDec
// Convert a binary string to a decimal double. Allows for decimal numbers.
// Example --> 101.11 --> convertBinToDec("101.11", 3, 2) = 5.75
double convertBinToDec(char *bin, int sizeLeft, int sizeRight) {
	double dec = 0;

	if(sizeLeft) {
		for(int i=0; i<sizeLeft; i++) {
			if(bin[i] == '.') {
				break;
			}
			dec += (bin[i] - 48.0) * pow(2, sizeLeft-i-1);
		}
	}

	if(sizeRight) {
		for(int i=0; i<sizeRight; i++) {
			dec += (bin[i+sizeLeft+1] - 48.0) / pow(2, i+1);
		}
	}

	return dec;
}

// convertHexToBin
// Convert a hexadecimal string to a binary string
void convertHexToBin(char *hex, char *bin, int size) {
	int index = 0;
	
	for(int i=0; i<size; i++) {
		char c = hex[i];
		switch(c) {
			case '0':
				bin[index++] = '0';
				bin[index++] = '0';
				bin[index++] = '0';
				bin[index++] = '0';
				break;
			case '1':
				bin[index++] = '0';
				bin[index++] = '0';
				bin[index++] = '0';
				bin[index++] = '1';
				break;
			case '2':
				bin[index++] = '0';
				bin[index++] = '0';
				bin[index++] = '1';
				bin[index++] = '0';
				break;
			case '3':
				bin[index++] = '0';
				bin[index++] = '0';
				bin[index++] = '1';
				bin[index++] = '1';
				break;
			case '4':
				bin[index++] = '0';
				bin[index++] = '1';
				bin[index++] = '0';
				bin[index++] = '0';
				break;
			case '5':
				bin[index++] = '0';
				bin[index++] = '1';
				bin[index++] = '0';
				bin[index++] = '1';
				break;
			case '6':
				bin[index++] = '0';
				bin[index++] = '1';
				bin[index++] = '1';
				bin[index++] = '0';
				break;
			case '7':
				bin[index++] = '0';
				bin[index++] = '1';
				bin[index++] = '1';
				bin[index++] = '1';
				break;
			case '8':
				bin[index++] = '1';
				bin[index++] = '0';
				bin[index++] = '0';
				bin[index++] = '0';
				break;
			case '9':
				bin[index++] = '1';
				bin[index++] = '0';
				bin[index++] = '0';
				bin[index++] = '1';
				break;
			case 'A':
			case 'a':
				bin[index++] = '1';
				bin[index++] = '0';
				bin[index++] = '1';
				bin[index++] = '0';
				break;
			case 'B':
			case 'b':
				bin[index++] = '1';
				bin[index++] = '0';
				bin[index++] = '1';
				bin[index++] = '1';
				break;
			case 'C':
			case 'c':
				bin[index++] = '1';
				bin[index++] = '1';
				bin[index++] = '0';
				bin[index++] = '0';
				break;
			case 'D':
			case 'd':
				bin[index++] = '1';
				bin[index++] = '1';
				bin[index++] = '0';
				bin[index++] = '1';
				break;
			case 'E':
			case 'e':
				bin[index++] = '1';
				bin[index++] = '1';
				bin[index++] = '1';
				bin[index++] = '0';
				break;
			case 'F':
			case 'f':
				bin[index++] = '1';
				bin[index++] = '1';
				bin[index++] = '1';
				bin[index++] = '1';
				break;
			default:
				cerr << "Unrecognized token: " << c << endl;
				break;
		}
	}
}

// convertIntToChar
// Convert a decimal digit to it's hexadecimal representation
char convertIntToChar(int toConvert) {
	switch(toConvert) {
		case 0:	return '0';
		case 1:	return '1';
		case 2:	return '2';
		case 3:	return '3';
		case 4:	return '4';
		case 5:	return '5';
		case 6:	return '6';
		case 7:	return '7';
		case 8:	return '8';
		case 9:	return '9';
		case 10: return 'a';
		case 11: return 'b';
		case 12: return 'c';
		case 13: return 'd';
		case 14: return 'e';
		case 15: return 'f';
		default: return -1;
	}
}

#endif
